﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using URWPGSim2D.Core;
using xna = Microsoft.Xna.Framework;
using Microsoft.Xna.Framework;
using URWPGSim2D.Common;
using URWPGSim2D.StrategyLoader;


namespace URWPGSim2D.FishDecision
{
    public static class FishDecision
    {
        #region 弧度转变为度
        public static float RedToDegree(float red)
        {
            float degree = red / (float)Math.PI * 180;
            return degree;
        }
        #endregion

        #region 获得鱼对目标的角度
        public static float GetFishToDestRad(Vector3 cur, Vector3 dest, float fishRad)
        {
            float fishtodestrad = 0;//接收目标对象和鱼的相对角度
            double tan = 0;  //接受正切值
            if (dest.X == cur.X)
            {                                //计算目标对象相当Fish[0]的相对角度bD,值为-PI~PI
                if (dest.Z > cur.Z)
                {
                    fishtodestrad = 90;
                }
                else
                {
                    fishtodestrad = -90;
                }
            }
            else if (dest.X - cur.X > 0)
            {
                tan = (dest.Z - cur.Z) / (dest.X - cur.X);
                fishtodestrad = RedToDegree((float)Math.Atan(tan));
            }
            else if (dest.Z >= cur.Z)
            {
                tan = (dest.Z - cur.Z) / (dest.X - cur.X);
                fishtodestrad = 180 + RedToDegree((float)Math.Atan(tan));
            }
            else
            {
                tan = (dest.Z - cur.Z) / (dest.X - cur.X);
                fishtodestrad = -180 + RedToDegree((float)Math.Atan(tan));
            }

            float xD = fishtodestrad - RedToDegree(fishRad);          //鱼应该转动的角度，正为顺时，负为逆时
            if (xD < -180)
            {
                xD += 360;
            }
            else if (xD > 180)
            {
                xD -= 360;
            }
            return xD;
        }
        #endregion

        #region 实现鱼转向目标的操作
        public static void FishTrun(Mission mission, ref Decision decision, int teamID, int fishID, Vector3 destinty)
        {
            float fishtodestrad = GetFishToDestRad(mission.TeamsRef[teamID].Fishes[fishID].PositionMm, destinty, mission.TeamsRef[teamID].Fishes[fishID].BodyDirectionRad);



            int tc = 14;            //给予初始的一定角速度排除球和鱼成角180的情况方向初始位顺时针
            int vc = 1;
            if (fishtodestrad > -10 && fishtodestrad < 10)
            {
                vc = 6;
            }
            if (fishtodestrad <= -90)
            {
                tc = 0;
            }
            else if (fishtodestrad < 0)
            {
                tc = 7 + (int)(fishtodestrad / (90 / 4)) - 3;  // 在-90~0度间最大速度为0最小为3
            }
            else if (fishtodestrad < 90)
            {
                tc = 7 + (int)(fishtodestrad / (90 / 4)) + 3;   // 在0~90度间最大速度为14最小为11
            }
            else
            {
                tc = 14;
            }
            decision.TCode = tc;
            decision.VCode = vc;

        }
        #endregion

        #region dribble算法
        /// <summary>
        /// dirbble带球算法 Modified By Zhangbo 2011.10.22
        /// </summary>
        /// <param name="decision">每周期机器鱼执行策略，包含速度档位，转弯档位。</param>
        /// <param name="fish">目标仿真机器鱼参数，包含当前位置、速度信息。</param>
        /// <param name="destPtMm">临时目标点。</param>
        /// <param name="destDirRad">目标方向。</param>
        /// <param name="angleTheta1">鱼体方向与目标方向角度差的阈值一。
        ///   角度差在此阈值范围内，则赋给机器鱼一个合理的速度档位（见参数disThreshold说明）。</param>
        /// <param name="angleTheta2">鱼体方向与目标方向角度差的阈值二。角度差小于此阈值，则机器鱼直线游动；
        /// 角度差大于此阈值，则机器鱼调整游动方向。</param>
        /// <param name="disThreshold">距离阈值。距离大于此阈值，机器鱼以速度档位VCode1游动；
        /// 距离小于此阈值，机器鱼以速度档位VCode2游动。</param>
        /// /// <param name="VCode1">直游档位1（默认6档）。</param>
        /// /// <param name="VCode2">直游档位2（默认4档）。</param>
        /// <param name="cycles">速度和转弯档位之间切换所需周期数经验值。建议取值范围在5-20之间。此参数作用是防止机器鱼“转过”。</param>
        /// <param name="msPerCycle">每个仿真周期的毫秒数，传递固定参数，不能修改。</param>
        /// <param name="flag">机器鱼坐标选择标准，true为PositionMm，即鱼体绘图中心；false为PolygonVertices[0]，即鱼头点。</param>
        public static void Dribble(ref Decision decision, RoboFish fish, xna.Vector3 destPtMm, float destDirRad,
            float angleTheta1, float angleTheta2, float disThreshold, int VCode1, int VCode2, int cycles, int msPerCycle, bool flag)
        {
            // 调节所用周期数及每周期毫秒数转换得到秒数
            double seconds1 = 15 * msPerCycle / 1000.0;
            double seconds2 = cycles * msPerCycle / 1000.0;
            // 标志量为true则起始点为PositionMm即鱼体绘图中心false则起始点为PolygonVertices[0]即鱼头点（起始点）
            xna.Vector3 srcPtMm = (flag == true) ? fish.PositionMm : fish.PolygonVertices[0];
            // 起始点到目标点的距离（目标距离）
            double disSrcPtMmToDestPtMm = Math.Sqrt(Math.Pow(destPtMm.X - srcPtMm.X, 2.0)
                + Math.Pow(destPtMm.Z - srcPtMm.Z, 2.0));

            // 鱼体绘图中心指向目标点向量方向的弧度值（中间方向）
            double dirFishToDestPtRad = xna.MathHelper.ToRadians((float)GetAngleDegree(destPtMm - fish.PositionMm));
            if (disSrcPtMmToDestPtMm < 0)
            {// 起始点到目标点距离小于阈值（默认58毫米）将中间方向调为目标方向
                dirFishToDestPtRad = destDirRad;
            }

            // 中间方向与鱼体方向的差值（目标角度）
            double deltaTheta = dirFishToDestPtRad - fish.BodyDirectionRad;
            // 将目标角度规范化到(-PI,PI]
            // 规范化之后目标角度为正表示目标方向在鱼体方向右边
            // 规范化之后目标角度为负表示目标方向在鱼体方向左边
            if (deltaTheta > Math.PI)
            {// 中间方向为正鱼体方向为负才可能目标角度大于PI
                deltaTheta -= 2 * Math.PI;  // 规范化到(-PI,0)
            }
            else if (deltaTheta < -Math.PI)
            {// 中间方向为负鱼体方向为正才可能目标角度小于-PI
                deltaTheta += 2 * Math.PI;  // 规范化到(0,PI)
            }

            // 最大角速度取左转和右转最大角速度绝对值的均值
            float maxAngularV = (Math.Abs(DataBasedOnExperiment.TCodeAndAngularVelocityTable[0])
                + Math.Abs(DataBasedOnExperiment.TCodeAndAngularVelocityTable[14])) / 2;
            // 以最大角速度转过目标角度所需的预计时间（角度预计时间）
            double estimatedTimeByAngle = Math.Abs((double)(deltaTheta / maxAngularV));
            // 以角度预计时间游过目标距离所需平均速度值（目标速度）
            double targetVelocity = disSrcPtMmToDestPtMm / estimatedTimeByAngle;

            int code = 1;   // 目标（速度）档位初值置1
            while ((code < 10) && (DataBasedOnExperiment.VCodeAndVelocityTable[code] < targetVelocity))
            {// 目标（速度）档位对应的速度值尚未达到目标速度则调高目标（速度）档位
                code++;
            }
            decision.VCode = code;
            if (Math.Abs(deltaTheta) > angleTheta2 * Math.PI / 180.0)
            {// 目标角度绝对值超过某一阈值，速度档位置次低进行小半径转弯
                decision.VCode = 1;
            }
            else if (Math.Abs(deltaTheta) < angleTheta1 * Math.PI / 180.0)
            {// 目标角度绝对值小于某一阈值，若此时距离较远速度档置较高高全速前进，否则置适中档位前进。
                if (disSrcPtMmToDestPtMm > disThreshold)
                {
                    decision.VCode = VCode1;
                }
                else
                {
                    decision.VCode = VCode2;
                }
            }


            // 以最大速度游过目标距离所需的预计时间（距离预计时间）
            double estimatedTimeByDistance = disSrcPtMmToDestPtMm / DataBasedOnExperiment.VCodeAndVelocityTable[14];
            if (estimatedTimeByDistance > seconds1)
            {// 距离预计时间超过一次档位切换所需平均时间则取为该时间（默认为1秒）
                estimatedTimeByDistance = seconds1;
            }
            // 以距离预计时间游过目标角度所需平均角速度（目标角速度）
            double targetAngularV = deltaTheta / estimatedTimeByDistance;

            code = 7;
            if (deltaTheta <= 0)
            {// 目标角度为负目标方向在鱼体方向左边需要给左转档位（注意左转档位对应的角速度值为负值）
                while ((code > 0) && (DataBasedOnExperiment.TCodeAndAngularVelocityTable[code] > targetAngularV))
                {// 目标（转弯）档位对应的角速度值尚未达到目标角速度则调低目标（转弯）档位
                    code = code - 2;
                }
                if ((fish.AngularVelocityRadPs * seconds2) < deltaTheta)
                {// 当前角速度值绝对值过大 一次档位切换所需平均时间内能游过的角度超过目标角度
                    // 则给相反方向次大转弯档位
                    code = 12;
                }
                if (code < 0)
                    code = 0;
            }
            else
            {// 目标角度为正目标方向在鱼体方向右边需要给右转档位
                while ((code < 14) && (DataBasedOnExperiment.TCodeAndAngularVelocityTable[code] < targetAngularV))
                {// 目标（转弯）档位对应的角速度值尚未达到目标角速度则调高目标（转弯）档位
                    code = code + 2;
                }
                if ((fish.AngularVelocityRadPs * seconds2) > deltaTheta)
                {// 当前角速度值绝对值过大 一次档位切换所需平均时间内能游过的角度超过目标角度
                    // 则给相反方向次大转弯档位
                    code = 2;
                }
                if (code > 14)
                    code = 14;
            }
            decision.TCode = code;
        }
        #endregion

        #region dribble2算法
        /// <summary>
        /// 
        /// </summary>
        /// <param name="decision">每周期机器鱼执行策略，包含速度档位，转弯档位。</param>
        /// <param name="fish">目标仿真机器鱼参数，包含当前位置、速度信息。</param>
        /// <param name="destPtMm">临时目标点。</param>
        /// <param name="angleTheta1">鱼体方向与目标方向角度差的阈值一。
        ///   角度差在此阈值范围内，则赋给机器鱼一个合理的速度档位（见参数disThreshold说明）。</param>
        /// <param name="angleTheta2">鱼体方向与目标方向角度差的阈值二。
        ///   角度差在此阈值范围内，则赋给机器鱼一个合理的次速度档位。</param>
        /// <param name="angleTheta3">鱼体方向与目标方向角度差的阈值三。角度差小于此阈值，则机器鱼直线游动；
        /// 角度差大于此阈值，则机器鱼调整游动方向。</param>
        /// <param name="disThreshold">距离阈值。距离大于此阈值，机器鱼以速度档位VCode1游动；
        /// 距离小于此阈值，机器鱼以速度档位VCode2游动。</param>
        /// /// <param name="VCode1">直游档位1（默认8档）。</param>
        /// /// <param name="VCode2">直游档位2（默认6档）。</param>
        /// /// <param name="VCode3">直游档位3（默认4档）。</param>
        /// <param name="cycles">速度和转弯档位之间切换所需周期数经验值。建议取值范围在5-20之间。此参数作用是防止机器鱼“转过”。</param>
        /// <param name="msPerCycle">每个仿真周期的毫秒数，传递固定参数，不能修改。</param>
        /// <param name="flag">机器鱼坐标选择标准，true为PositionMm，即鱼体绘图中心；false为PolygonVertices[0]，即鱼头点。</param>
        public static void Dribble2(ref Decision decision, RoboFish fish, xna.Vector3 destPtMm,
            float angleTheta1, float angleTheta2, float angleTheta3, float disThreshold, int VCode1, int VCode2, int VCode3, int cycles, int msPerCycle, bool flag)
        {
            // 调节所用周期数及每周期毫秒数转换得到秒数
            double seconds1 = 15 * msPerCycle / 1000.0;
            double seconds2 = cycles * msPerCycle / 1000.0;
            // 标志量为true则起始点为PositionMm即鱼体绘图中心false则起始点为PolygonVertices[0]即鱼头点（起始点）
            xna.Vector3 srcPtMm = (flag == true) ? fish.PositionMm : fish.PolygonVertices[0];
            // 起始点到目标点的距离（目标距离）
            double disSrcPtMmToDestPtMm = Math.Sqrt(Math.Pow(destPtMm.X - srcPtMm.X, 2.0)
                + Math.Pow(destPtMm.Z - srcPtMm.Z, 2.0));

            // 鱼体绘图中心指向目标点向量方向的弧度值（中间方向）
            double dirFishToDestPtRad = xna.MathHelper.ToRadians((float)GetAngleDegree(destPtMm - fish.PositionMm));


            // 中间方向与鱼体方向的差值（目标角度）
            double deltaTheta = dirFishToDestPtRad - fish.BodyDirectionRad;
            // 将目标角度规范化到(-PI,PI]
            // 规范化之后目标角度为正表示目标方向在鱼体方向右边
            // 规范化之后目标角度为负表示目标方向在鱼体方向左边
            if (deltaTheta > Math.PI)
            {// 中间方向为正鱼体方向为负才可能目标角度大于PI
                deltaTheta -= 2 * Math.PI;  // 规范化到(-PI,0)
            }
            else if (deltaTheta < -Math.PI)
            {// 中间方向为负鱼体方向为正才可能目标角度小于-PI
                deltaTheta += 2 * Math.PI;  // 规范化到(0,PI)
            }

            // 最大角速度取左转和右转最大角速度绝对值的均值
            float maxAngularV = (Math.Abs(DataBasedOnExperiment.TCodeAndAngularVelocityTable[0])
                + Math.Abs(DataBasedOnExperiment.TCodeAndAngularVelocityTable[14])) / 2;
            // 以最大角速度转过目标角度所需的预计时间（角度预计时间）
            double estimatedTimeByAngle = Math.Abs((double)(deltaTheta / maxAngularV));
            // 以角度预计时间游过目标距离所需平均速度值（目标速度）
            double targetVelocity = disSrcPtMmToDestPtMm / estimatedTimeByAngle;

            int code = 1;   // 目标（速度）档位初值置1
            while ((code < 10) && (DataBasedOnExperiment.VCodeAndVelocityTable[code] < targetVelocity))
            {// 目标（速度）档位对应的速度值尚未达到目标速度则调高目标（速度）档位
                code++;
            }
            decision.VCode = code;


            if (Math.Abs(deltaTheta) < angleTheta1 * Math.PI / 180.0)
            {// 目标角度绝对值小于某一阈值，若此时距离较远速度档置较高高全速前进，否则置适中档位前进。
                if (disSrcPtMmToDestPtMm < disThreshold)
                {
                    decision.VCode = VCode1;
                }
                else
                {
                    decision.VCode = VCode1 + 3;
                }
                //decision.VCode = VCode1;
            }
            else if (Math.Abs(deltaTheta) < angleTheta2 * Math.PI / 180.0)
            {
                if (disSrcPtMmToDestPtMm < disThreshold)
                {
                    decision.VCode = VCode2;
                }
                else
                {
                    decision.VCode = VCode2 + 2;
                }
            }
            else if (Math.Abs(deltaTheta) < angleTheta3 * Math.PI / 180.0)
            {
                if (disSrcPtMmToDestPtMm < disThreshold)
                {
                    decision.VCode = VCode2;
                }
                else
                {
                    decision.VCode = VCode2 + 1;
                }
            }
            else
            {// 目标角度绝对值超过某一阈值，速度档位置次低进行小半径转弯
                decision.VCode = 1;
            }

            // 以最大速度游过目标距离所需的预计时间（距离预计时间）
            double estimatedTimeByDistance = disSrcPtMmToDestPtMm / DataBasedOnExperiment.VCodeAndVelocityTable[14];
            if (estimatedTimeByDistance > seconds1)
            {// 距离预计时间超过一次档位切换所需平均时间则取为该时间（默认为1秒）
                estimatedTimeByDistance = seconds1;
            }
            // 以距离预计时间游过目标角度所需平均角速度（目标角速度）
            double targetAngularV = deltaTheta / estimatedTimeByDistance;

            code = 7;
            if (deltaTheta <= 0)
            {// 目标角度为负目标方向在鱼体方向左边需要给左转档位（注意左转档位对应的角速度值为负值）
                while ((code > 0) && (DataBasedOnExperiment.TCodeAndAngularVelocityTable[code] > targetAngularV))
                {// 目标（转弯）档位对应的角速度值尚未达到目标角速度则调低目标（转弯）档位
                    code = code - 1;
                }
                if ((fish.AngularVelocityRadPs * seconds2) < deltaTheta)
                {// 当前角速度值绝对值过大 一次档位切换所需平均时间内能游过的角度超过目标角度
                    // 则给相反方向次大转弯档位
                    code = 12;
                }
                if (code < 0)
                    code = 0;
            }
            else
            {// 目标角度为正目标方向在鱼体方向右边需要给右转档位
                while ((code < 14) && (DataBasedOnExperiment.TCodeAndAngularVelocityTable[code] < targetAngularV))
                {// 目标（转弯）档位对应的角速度值尚未达到目标角速度则调高目标（转弯）档位
                    code = code + 1;
                }
                if ((fish.AngularVelocityRadPs * seconds2) > deltaTheta)
                {// 当前角速度值绝对值过大 一次档位切换所需平均时间内能游过的角度超过目标角度
                    // 则给相反方向次大转弯档位
                    code = 2;
                }
                if (code > 14)
                    code = 14;
            }
            decision.TCode = code;
        }
        #endregion

        #region Dribble2打包
        public static void Dribble3(ref Decision decision, RoboFish fish, xna.Vector3 goal, xna.Vector3 ball)
        {
            float angleTheta1 = 7;
            float angleTheta2 = 4;
            float angleTheta3 = 1;
            float disThreshold = 100;
            int VCode1 = 5;
            int VCode2 = 4;
            int VCode3 = 3;
            int cycles = 10;
            int msPerCycle = 100;
            bool flag = true;
            int degree = (int)GetFishToDestRad(fish.PositionMm, goal, fish.BodyDirectionRad);
            Vector3 setpoint = SetDestPtMm(goal, ball, degree, 40);
            Dribble2(ref decision, fish, setpoint, angleTheta1, angleTheta2, angleTheta3, disThreshold, VCode1, VCode2, VCode3, cycles, msPerCycle, flag);
        }

        #endregion

        #region 获得角度
        /// <summary>
        /// 返回Vector3类型的向量（Y置0，只有X和Z有意义）在场地坐标系中方向的角度值
        /// 场地坐标系定义为：X向右，Z向下，Y置0，负X轴顺时针转回负X轴角度范围为(-PI,PI)的坐标系
        /// </summary>
        /// <param name="v">待计算角度值的xna.Vector3类型向量</param>
        /// <returns>向量v在场地坐标系中方向的角度值</returns>
        public static float GetAngleDegree(xna.Vector3 v)
        {
            float x = v.X;
            float y = v.Z;
            float angle = 0;

            if (Math.Abs(x) < float.Epsilon)
            {// x = 0 直角反正切不存在
                if (Math.Abs(y) < float.Epsilon) { angle = 0.0f; }
                else if (y > 0) { angle = 90.0f; }
                else if (y < 0) { angle = -90.0f; }
            }
            else if (x < 0)
            {// x < 0 (90,180]或(-180,-90)
                if (y >= 0) { angle = (float)(180 * Math.Atan(y / x) / Math.PI) + 180.0f; }
                else { angle = (float)(180 * Math.Atan(y / x) / Math.PI) - 180.0f; }
            }
            else
            {// x > 0 (-90,90)
                angle = (float)(180 * Math.Atan(y / x) / Math.PI);
            }

            return angle;
        }
        #endregion

        #region 获得临时目标点
        /// <summary>
        /// 
        /// </summary>
        /// <param name="goal">为目标坐标</param>
        /// <param name="ball">为目标球的坐标</param>
        /// <param name="setDegree">设置偏转角度</param>
        /// <param name="setDist">为设置的临时目标点相距球的距离 一般30~45</param>
        /// <returns></returns>

        public static Vector3 SetDestPtMm(xna.Vector3 goal, xna.Vector3 ball, int setDegree, int setDist)
        {
            //求 洞指向球 的单位向量
            double xx = (ball.X - goal.X) / Math.Pow(((goal.X - ball.X) * (goal.X - ball.X) + (goal.Z - ball.Z) * (goal.Z - ball.Z)), 0.5);
            double zz = (ball.Z - goal.Z) / Math.Pow(((goal.X - ball.X) * (goal.X - ball.X) + (goal.Z - ball.Z) * (goal.Z - ball.Z)), 0.5);
            float rad = (float)(setDegree * Math.PI / 180);
            double tx = xx * Math.Cos(rad) - zz * Math.Sin(rad);
            double tz = xx * Math.Sin(rad) + zz * Math.Cos(rad);
            Vector3 p = new Vector3();
            p.X = ball.X + (float)tx * setDist;
            p.Z = ball.Z + (float)tz * setDist;
            return p;
        }
        #endregion

        #region 获得目标方向
        public static float GetDirRad(xna.Vector3 cur, xna.Vector3 dest)
        {
            float dirRad = 0;//接收目标对象和鱼的相对角度
            double tan = 0;  //接受正切值
            if (dest.X == cur.X)
            {                                //计算目标对象相当Fish[0]的相对角度bD,值为-PI~PI
                if (dest.Z > cur.Z)
                {
                    dirRad = (float)Math.PI / 2;
                }
                else
                {
                    dirRad = -(float)Math.PI / 2;
                }
            }
            else if (dest.X - cur.X > 0)
            {
                tan = (dest.Z - cur.Z) / (dest.X - cur.X);
                dirRad = (float)Math.Atan(tan);
            }
            else if (dest.Z >= cur.Z)
            {
                tan = (dest.Z - cur.Z) / (dest.X - cur.X);
                dirRad = (float)Math.PI + (float)Math.Atan(tan);
            }
            else
            {
                tan = (dest.Z - cur.Z) / (dest.X - cur.X);
                dirRad = -(float)Math.PI + (float)Math.Atan(tan);
            }

            return dirRad;

        }
        #endregion

        #region 判断是否在可带球范围
        /// <summary>
        /// 
        /// </summary>
        /// <param name="fish">动作鱼</param>
        /// <param name="ball">目标球</param>
        /// <param name="dest">目标点</param>
        /// <param name="dis">偏置距离，一般是球心后100左右</param>
        /// <param name="radius">范围半径一般为60</param>
        /// <returns></returns>
        public static bool JudgeArea(RoboFish fish, xna.Vector3 ball, xna.Vector3 dest, int setDegree, int dis, int radius)
        {
            float fX = fish.PositionMm.X;
            float fZ = fish.PositionMm.Z;
            Vector3 area = new Vector3();
            area = SetDestPtMm(dest, ball, setDegree, dis);
            float xm = area.X;
            float zm = area.Z;
            if ((fX - xm) * (fX - xm) + (fZ - zm) * (fZ - zm) <= radius * radius)
            {
                return true;
            }
            else
            {
                return false;
            }

        }
        #endregion

        #region 计算鱼所在位置到目标球的角度于目标角度的差
        public static float GetFishAtBallAngleDegree(xna.Vector3 dest, xna.Vector3 ball, xna.Vector3 fish)
        {
            float AngleDegree = 0;
            float rad1 = GetDirRad(dest, ball);
            float rad2 = GetDirRad(fish, ball);
            double Trad = rad2 - rad1;
            if (Trad > Math.PI)
            {
                Trad -= 2 * Math.PI;
            }
            else if (Trad < -Math.PI)
            {
                Trad += 2 * Math.PI;
            }
            AngleDegree = (float)(Trad / Math.PI * 180);
            return AngleDegree;
        }
        #endregion

        #region 计算两点距离
        public static double GetDistBetwGB(xna.Vector3 v1, xna.Vector3 v2)
        {
            return Math.Pow((v1.X - v2.X) * (v1.X - v2.X) + (v1.Z - v2.Z) * (v1.Z - v2.Z), 0.5);
        }
        #endregion

        #region 判断区域
        /// <summary>
        /// 判断目标是否在指定区域内，全场氛围10个区域，整数1-10分别对应相关区域
        /// </summary>
        /// <param name="p">目标坐标</param>
        /// <param name="area">区域编号</param>
        /// <returns>bool</returns>
        public static bool JudgePIsInArea(xna.Vector3 p, int area)
        {
            switch (area)
            {
                case 1:
                    if (p.X <= -950 && p.Z < -560)
                    {
                        return true;
                    }
                    else
                    {
                        return false;
                    }
                case 2:
                    if (p.X <= -950 && p.Z <= 560 && p.Z >= -560)
                    {
                        return true;
                    }
                    else
                    {
                        return false;
                    }
                case 3:
                    if (p.X <= -950 && p.Z > 560)
                    {
                        return true;
                    }
                    else
                    {
                        return false;
                    }
                case 4:
                    if (p.X >= 950 && p.Z < -560)
                    {
                        return true;
                    }
                    else
                    {
                        return false;
                    }
                case 5:
                    if (p.X >= 950 && p.Z >= -560 && p.Z <= 560)
                    {
                        return true;
                    }
                    else
                    {
                        return false;
                    }
                case 6:
                    if (p.X >= 950 && p.Z > 560)
                    {
                        return true;
                    }
                    else
                    {
                        return false;
                    }
                case 7:
                    if (p.X > -950 && p.X < 950 && p.Z < -560)
                    {
                        return true;
                    }
                    else
                    {
                        return false;
                    }
                case 8:
                    if (p.X > -950 && p.X < 950 && p.Z > -560 && p.Z < 0)
                    {
                        return true;
                    }
                    else
                    {
                        return false;
                    }
                case 9:
                    if (p.X > -950 && p.X < 950 && p.Z < 560 && p.Z > 0)
                    {
                        return true;
                    }
                    else
                    {
                        return false;
                    }
                case 10:
                    if (p.X > -950 && p.X < 950 && p.Z > 560)
                    {
                        return true;
                    }
                    else
                    {
                        return false;
                    }
                default:
                    return false;
            }
        }
        #endregion

        #region 判断鱼所在区域
        public static int FishArea(xna.Vector3 fish)
        {
            int area = 0;
            bool fish1 = JudgePIsInArea(fish, 1);
            if (fish1 == true)
            {
                area = 1;
            }
            bool fish2 = JudgePIsInArea(fish, 2);
            if (fish2 == true)
            {
                area = 2;
            }
            bool fish3 = JudgePIsInArea(fish, 3);
            if (fish3 == true)
            {
                area = 3;
            }
            bool fish4 = JudgePIsInArea(fish, 4);
            if (fish4 == true)
            {
                area = 4;
            }
            bool fish5 = JudgePIsInArea(fish, 5);
            if (fish5 == true)
            {
                area = 5;
            }
            bool fish6 = JudgePIsInArea(fish, 6);
            if (fish6 == true)
            {
                area = 6;
            }
            bool fish7 = JudgePIsInArea(fish, 7);
            if (fish7 == true)
            {
                area = 7;
            }
            bool fish8 = JudgePIsInArea(fish, 8);
            if (fish8 == true)
            {
                area = 8;
            }
            bool fish9 = JudgePIsInArea(fish, 9);
            if (fish9 == true)
            {
                area = 9;
            }
            bool fish10 = JudgePIsInArea(fish, 10);
            if (fish10 == true)
            {
                area = 10;
            }
            return area;
        }
        #endregion

        #region 设置定角方向偏置
        /// <summary>
        /// 设置定角方向偏置,获得固定方向，实现鱼的固定方向游动
        /// </summary>
        /// <param name="fish">鱼的坐标</param>
        /// <param name="degree">偏转的角度（+为顺时针，-为逆时针）</param>
        /// <returns>Vector3</returns>
        public static Vector3 SetDegreeFishToP(xna.Vector3 fish, int degree)
        {
            xna.Vector3 p = new Vector3();
            float rad = (float)(degree * Math.PI / 180);
            double tx = 100 * Math.Cos(rad) - 0 * Math.Sin(rad);
            double tz = 100 * Math.Sin(rad) + 0 * Math.Cos(rad);
            p.X = fish.X + (float)tx;
            p.Z = fish.Z + (float)tz;
            return p;
        }
        #endregion

        #region Dribble_Will封装
        /// <summary>
        /// Dribble_Will封装
        /// </summary>
        /// <param name="mission">mission</param>
        /// <param name="decision">ref decision</param>
        /// <param name="teamId">teamId</param>
        /// <param name="fishId">fishId</param>
        /// <param name="ballId">目标球编号</param>
        /// <param name="goal">目标洞坐标</param>
        public static void Dribble_Will(Mission mission, ref Decision decision, int teamId, int fishId, int ballId, xna.Vector3 goal,
             float angleTheta1, float angleTheta2, float angleTheta3, int VCode1, int VCode2, int VCode3)
        {
            //接收球的坐标
            xna.Vector3 bM = mission.EnvRef.Balls[ballId].PositionMm;
            //接收鱼的坐标      
            xna.Vector3 fM = mission.TeamsRef[teamId].Fishes[fishId].PositionMm;
            //防止反带球绕后///////////
            #region 绕后处理
            ////////////如果在鱼驶向目标球后方时，球会被误撞，就给他给他一个偏转角，从侧面过去//////////////////////////////////
            int Tdegree = 0;  //根据情况设置不同的偏置角度
            if (GetFishAtBallAngleDegree(goal, bM, fM) < 80 && GetFishAtBallAngleDegree(goal, bM, fM) > 0)
            {
                Tdegree = -60;
            }
            else if (GetFishAtBallAngleDegree(goal, bM, fM) > -80 && GetFishAtBallAngleDegree(goal, bM, fM) < 0)
            {
                Tdegree = 60;
            }
            #endregion


            //临时目标点//////////////////////////////////处理后置偏转的问题/////////////////////////////////////////////////
            #region 临时目标点
            /////////////////////当球靠近洞口的时候，临时目标点的偏置变量后向后推，经验值51//////////////////////////////////
            xna.Vector3 P = new xna.Vector3();//////存放临时目标点
            int setD = 40;                  ///////偏执变量
                                            //    if ((bM.X > -110 && bM.X < 110) &&
                                            //        (bM.Z < -850 || bM.Z > 850) ||
                                            //        (GetDistBetwGB(bM, goal) <= 550) && goal.X == 0 ||
                                            //        ballId == 7
                                            //        )
                                            //    {
                                            //        setD = 51;
                                            //        //当目标球在左半场且距离洞口很近的时候，偏置放大，防止定过
                                            //        if (bM.X < -250 && goal.X == 0 ||
                                            //            Math.Abs(bM.Z) > 1200)
                                            //        {
                                            //            setD = 61;
                                            //        }
                                            //
                                            //        if (bM.Z > 1330 || bM.Z < -1330)
                                            //        {
                                            //            setD = 10;
                                            //        }
                                            //    }
                                            //    else
                                            //    {
                                            //        setD = 23;
                                            //    }
            #endregion

            //生成临时目标点
            P = SetDestPtMm(goal, bM, 0, setD);


            //判断鱼是否在球后
            bool judge = JudgeArea(mission.TeamsRef[teamId].Fishes[fishId], bM, goal, Tdegree, 250, 250);

            #region Dribble调用

            if (judge)//////判断是否在可顶球的范围
            {
                //         if (goal.X == 0) //中袋
                //         {
                //             if (ballId == 0 && bM.Z < 600)
                //             {
                //
                //                 Dribble2(ref decision, mission.TeamsRef[teamId].Fishes[fishId], P, 2, 4, 6, 180, 8, 5, 4, 10, 100, true);
                //             }
                //
                Dribble2(ref decision, mission.TeamsRef[teamId].Fishes[fishId], P, angleTheta1, angleTheta2, angleTheta3, 200, VCode1, VCode2, VCode3, 10, 100, true);
                //         }
                //
                //         else //底袋
                //         {
                //
                //
                //             Dribble2(ref decision, mission.TeamsRef[teamId].Fishes[fishId], P, 4, 6, 7, 180, 8, 6, 4, 10, 100, true);
                //             // fish.FishDecision.Dribble(ref decisions[0], mission.TeamsRef[teamId].Fishes[0], P, dirRad, 3, 5, 150, 9, 5, 10, 100, true);
                //         }
                //         //    if (judge2) 
                //         //    {
                //         //        Vector3 tP = fish.FishDecision.SetDestPtMm(goal, mission.EnvRef.Balls[index].PositionMm, 0, 500);
                //         //        fish.FishDecision.Dribble2(ref decisions[0], mission.TeamsRef[teamId].Fishes[0] ,tP, 4, 6, 8, 180, 7, 6, 4, 10, 100, true);
                //         //    }
                //
                //         //中袋洞口处，鱼加速 
                //         if ((bM.Z < -1330 || bM.Z > 1330) && (bM.X > -45 && bM.X < 45))
                //         {
                //             Dribble2(ref decision, mission.TeamsRef[teamId].Fishes[fishId], P, 4, 6, 9, 300, 13, 10, 4, 10, 100, true);
                //         }
                //
                //        //边带带球加速
                //         else if ((bM.Z < -1150 || bM.Z > 1150) && (bM.X > 2140 || bM.X < -2140) ||
                //             (bM.Z < -1200 || bM.Z > 1200) && (bM.X > 1950 || bM.X < -1950)
                //             )
                //         {
                //             Dribble2(ref decision, mission.TeamsRef[teamId].Fishes[fishId], P, 4, 6, 9, 300, 13, 10, 4, 10, 100, true);
                //         }
            }
            else  ////距离远时，全速驶向球后
            {

                int Dis = 300;
                //如果球就在洞口则直接冲向球
                //if (bM.Z > 1350 || bM.Z < -1350)
                //{
                //    Dis = 10;
                //}



                ///////开球偏置//////////////////////////////////////////////////////////////////////////////
                //if (ballId == 0 && mission.TeamsRef[teamId].Fishes[fishId].PositionMm.X < -360)
                //{
                //    Dis = 600;
                //    Tdegree = -65;
                //}

                //         fish.FishDecision.Dribble2(
                //              ref decisions[0], mission.TeamsRef[teamId].Fishes[0],
                //              fish.FishDecision.SetDestPtMm(goal, bM, Tdegree, Dis),
                //              6, 11, 30, 1000, 10, 9, 8, 10, 100, true);
                Dribble(
                     ref decision, mission.TeamsRef[teamId].Fishes[fishId],
                     SetDestPtMm(goal, bM, Tdegree, Dis),
                     GetDirRad(mission.TeamsRef[teamId].Fishes[fishId].PositionMm,
                     SetDestPtMm(goal, bM, Tdegree, Dis)) + 3.14f,
                     20, 50, 800, 14, 10, 10, 100, true);

                /////////////////防止尾巴扫球////////////////////////////////////////////////////////
                //         if (bN[index] == 9)
                //         {
                //             if (mission.TeamsRef[teamId].Fishes[0].PositionMm.X > -450)
                //             {
                //                 Vector3 tb = new Vector3(-600, 0, 1000);
                //                 fish.FishDecision.Dribble(
                //                  ref decisions[0], mission.TeamsRef[teamId].Fishes[0],
                //                  fish.FishDecision.SetDestPtMm(goal, tb, Tdegree, 0),
                //                  fish.FishDecision.GetDirRad(mission.TeamsRef[teamId].Fishes[0].PositionMm,
                //                  fish.FishDecision.SetDestPtMm(goal, tb, Tdegree, 0)) + 3.14f,
                //                  10, 40, 800, 9, 4, 10, 100, true);
                //             }
                //         }
                ///===================================================================////
            }
            #endregion


        }
        #endregion

        #region Dribble_Will简化1（确定角度与速度）
        public static void Dribble_Will1(Mission mission, ref Decision decision, int teamId, int fishId, int ballId, xna.Vector3 goal)
        {
            float angleTheta1 = 7;
            float angleTheta2 = 4;
            float angleTheta3 = 1;
            int VCode1 = 7;
            int VCode2 = 7;
            int VCode3 = 7;
            Dribble_Will(mission, ref decision, teamId, fishId, ballId, goal, angleTheta1, angleTheta2, angleTheta3, VCode1, VCode2, VCode3);

        }
        #endregion

        #region Dribble_Will简化2（确定角度）
        public static void Dribble_Will2(Mission mission, ref Decision decision, int teamId, int fishId, int ballId, xna.Vector3 goal, int VCode1, int VCode2, int VCode3)
        {
            float angleTheta1 = 7;
            float angleTheta2 = 4;
            float angleTheta3 = 1;
            Dribble_Will(mission, ref decision, teamId, fishId, ballId, goal, angleTheta1, angleTheta2, angleTheta3, VCode1, VCode2, VCode3);

        }
        #endregion

        #region 移动到定点并保持姿态
        public static void MoveToAreaS(Mission mission, ref Decision decision, int teamId, int fishId, xna.Vector3 goal, int degree)
        {
            float degreeF = (float)(mission.TeamsRef[teamId].Fishes[fishId].BodyDirectionRad / Math.PI * 180);
            xna.Vector3 fM = mission.TeamsRef[teamId].Fishes[fishId].PositionMm;
            if (GetDistBetwGB(fM, goal) < 150)
            {
                xna.Vector3 p = SetDegreeFishToP(fM, degree);
                Dribble2(ref decision, mission.TeamsRef[teamId].Fishes[fishId], p, 60, 150, 180, 200, 12,12, 3, 10, 100, true);
                if (Math.Abs(degree - degreeF) <= 15)
                {
                    decision.TCode = 7;
                    decision.VCode = 0;
                }
            }
            else if (Math.Abs(degree - degreeF) <= 15 && GetDistBetwGB(fM, goal) < 200)
            {
                decision.TCode = 7;
                decision.VCode = 0;
            }
            else
            {
                Dribble2(ref decision, mission.TeamsRef[teamId].Fishes[fishId], goal, 9, 20, 60, 200, 7, 5, 2, 10, 100, true);
            }

        }
        #endregion

        #region 移动到定点并保持姿态2
        public static void MoveToAreaS2(Mission mission, ref Decision decision, int teamId, int fishId, xna.Vector3 goal, int degree)
        {
            float degreeF = (float)(mission.TeamsRef[teamId].Fishes[fishId].BodyDirectionRad / Math.PI * 180);
            xna.Vector3 fM = mission.TeamsRef[teamId].Fishes[fishId].PositionMm;
            if (GetDistBetwGB(fM, goal) < 150)
            {
                xna.Vector3 p = SetDegreeFishToP(fM, degree);
                Dribble2(ref decision, mission.TeamsRef[teamId].Fishes[fishId], p, 60, 150, 180, 200, 1, 1, 3, 10, 100, true);
                if (Math.Abs(degree - degreeF) <= 15)
                {
                    decision.TCode = 7;
                    decision.VCode = 0;
                }
            }
            else if (Math.Abs(degree - degreeF) <= 15 && GetDistBetwGB(fM, goal) < 200)
            {
                decision.TCode = 7;
                decision.VCode = 0;
            }
            else
            {
                Dribble2(ref decision, mission.TeamsRef[teamId].Fishes[fishId], goal, 9, 20, 60, 200, 7, 5, 2, 10, 100, true);
            }

        }
        #endregion

        #region 得到鱼体内一点坐标
        /// <summary>
        /// 得到鱼体内中轴线上一点坐标
        /// </summary>
        /// <param name="mission">mission</param>
        /// <param name="teamId">teamId</param>
        /// <param name="fishId">fishId</param>
        /// <param name="setD">偏置量（从鱼几何中心反向鱼头的距离，‘-’为向鱼头，‘+’相反）</param>
        /// <returns>Vector3</returns>
        public static Vector3 GetFishBodyPM(Mission mission, int teamId, int fishId, int setD)
        {
            Vector3 head = mission.TeamsRef[teamId].Fishes[fishId].PolygonVertices[0];
            Vector3 bodyIn = mission.TeamsRef[teamId].Fishes[fishId].PositionMm;
            return SetDestPtMm(head, bodyIn, 0, setD);
        }
        #endregion

        #region 鱼与球之间的距离
        public static float Distance(xna.Vector3 point0, xna.Vector3 point1)
        {
            float distance;
            distance = (float)Math.Sqrt(((point0.X - point1.X) * (point0.X - point1.X)) + ((point0.Z - point1.Z) * (point0.Z - point1.Z)));
            return distance;
        }
        #endregion

        #region 求六个数最小值
        public static float ShortestDistance6(float a, float b, float c, float d, float e, float f)
        {
            float g;
            if (a < b)
            {
                g = a;
            }
            else
            {
                g = b;
            }
            if (g > c)
            {
                g = c;
            }
            if (g > d)
            {
                g = d;
            }
            if (g > e)
            {
                g = e;
            }
            if (g > f)
            {
                g = f;
            }
            return g;
        }
        #endregion

        #region 求十个数的最小值
        public static float ShortestDistance10(float a, float b, float c, float d, float e, float f, float g, float h, float i, float j)
        {
            float k;
            if (a < b)
            {
                k = a;
            }
            else
            {
                k = b;
            }
            if (k > c)
            {
                k = c;
            }
            if (k > d)
            {
                k = d;
            }
            if (k > e)
            {
                k = e;
            }
            if (k > f)
            {
                k = f;
            }
            if (k > g)
            {
                k = g;
            }
            if (k > h)
            {
                k = h;
            }
            if (k > i)
            {
                k = i;
            }
            if (k > j)
            {
                k = j;
            }
            return k;
        }
        #endregion

        #region 输出最近目标
        public static int Target(float a, float b, float c, float d)
        {
            int ball = 0;
            if (a == b)
            {
                ball = 0;
            }
            if (a == c)
            {
                ball = 1;
            }
            if (a == d)
            {
                ball = 2;
            }
            return ball;
        }

        #endregion

        #region 判断左边最近的球是否进过
        public static bool Ifget(int b0_l, int b1_l, int b2_l, int b3_l, int b4_l, int b5_l, int b6_l, int b7_l, int b8_l, int ball)
        {
            if (ball == 0)
            {
                if (b0_l == 1)
                {
                    return true;
                }
                else
                {
                    return false;
                }
            }
            else if (ball == 1)
            {
                if (b1_l == 1)
                {
                    return true;
                }
                else
                {
                    return false;
                }
            }
            else if (ball == 2)
            {
                if (b2_l == 1)
                {
                    return true;
                }
                else
                {
                    return false;
                }
            }
            else if (ball == 3)
            {
                if (b3_l == 1)
                {
                    return true;
                }
                else
                {
                    return false;
                }
            }
            else if (ball == 4)
            {
                if (b4_l == 1)
                {
                    return true;
                }
                else
                {
                    return false;
                }
            }
            else if (ball == 5)
            {
                if (b5_l == 1)
                {
                    return true;
                }
                else
                {
                    return false;
                }
            }
            else if (ball == 6)
            {
                if (b6_l == 1)
                {
                    return true;
                }
                else
                {
                    return false;
                }
            }
            else if (ball == 7)
            {
                if (b7_l == 1)
                {
                    return true;
                }
                else
                {
                    return false;
                }
            }
            else if (ball == 8)
            {
                if (b8_l == 1)
                {
                    return true;
                }
                else
                {
                    return false;
                }
            }
            else
            {
                return false;
            }

        }

        #endregion

        #region 区域二进球
        public static void SelfCoutrol2(xna.Vector3 fish, xna.Vector3 ball, Mission mission, ref Decision decision, int teamId, int fishId, RoboFish fish1, int ballId)
        {
            float balltarget = ((-1040 - ball.X) * (fish.Z - ball.Z) / (fish.X - ball.X)) + ball.X;
            if ((ball.X - fish.X) * (-1040 - fish.X) >= 0)
            {
                if (balltarget > -440 && balltarget < 440)
                {
                    Dribble3(ref decision, fish1, new Vector3(-1040, 0, balltarget), ball);
                }
                else if (balltarget <= -440)
                {
                    Dribble3(ref decision, fish1, new Vector3(-1040, 0, -440), ball);
                }
                else if (balltarget >= 440)
                {
                    Dribble3(ref decision, fish1, new Vector3(-1040, 0, 440), ball);
                }
            }
            else
            {
                if (balltarget >= 440)
                {
                    Dribble3(ref decision, fish1, new Vector3(-1040, 0, ball.Z), ball);
                }
                else if (balltarget <= -440)
                {
                    Dribble3(ref decision, fish1, new Vector3(-1040, 0, ball.Z), ball);
                }
            }
        }
        #endregion

        #region 区域五进球
        public static void SelfCoutrol5(xna.Vector3 fish, xna.Vector3 ball, Mission mission, ref Decision decision, int teamId, int fishId, RoboFish fish1, int ballId)
        {
            float balltarget = ((1040 - ball.X) * (fish.Z - ball.Z) / (fish.X - ball.X)) + ball.X;
            if ((ball.X - fish.X) * (-1040 - fish.X) >= 0)
            {
                if (balltarget > -440 && balltarget < 440)
                {
                    Dribble3(ref decision, fish1, new Vector3(1040, 0, balltarget), ball);
                }
                else if (balltarget <= -440)
                {
                    Dribble3(ref decision, fish1, new Vector3(1040, 0, -440), ball);
                }
                else if (balltarget >= 440)
                {
                    Dribble3(ref decision, fish1, new Vector3(1040, 0, 440), ball);
                }
            }
            else
            {
                if (balltarget >= 440)
                {
                    Dribble3(ref decision, fish1, new Vector3(1040, 0, ball.Z), ball);
                }
                else if (balltarget <= -440)
                {
                    Dribble3(ref decision, fish1, new Vector3(1040, 0, ball.Z), ball);
                }
            }
        }
        #endregion

        #region 右半场下半场复位
        public static void RightRestart(Mission mission, ref Decision decision, int teamId, int fishId, bool judgefish01, bool judgefish02, bool judgefish03,
            bool judgefish04, bool judgefish05, bool judgefish06, bool judgefish07, bool judgefish08, bool judgefish09, bool judgefish10, xna.Vector3 fish)
        {
            if (judgefish01 == true)
            {
                FishDecision.MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(0, 0, 0), 0);
            }
            if (judgefish02 == true)
            {
                if (fish.Z < 0)
                {
                    FishDecision.MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-1300, 0, -700), 0);
                }
                else
                {
                    FishDecision.MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-1300, 0, 700), 0);
                }
            }
            if (judgefish03 == true)
            {
                FishDecision.MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(0, 0, 0), 0);
            }
            if (judgefish04 == true)
            {
                FishDecision.MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(0, 0, 0), 0);
            }
            if (judgefish05 == true)
            {
                if (fish.Z < 0)
                {
                    FishDecision.MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(1300, 0, -700), 0);
                }
                else
                {
                    FishDecision.MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(1300, 0, 700), 0);
                }
            }
            if (judgefish06 == true)
            {
                FishDecision.MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(0, 0, 0), 0);
            }
            if (judgefish07 == true)
            {
                FishDecision.MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(0, 0, 0), 0);
            }
            if (judgefish08 == true)
            {
                FishDecision.MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(0, 0, 0), 0);
            }
            if (judgefish09 == true)
            {
                FishDecision.MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(0, 0, 0), 0);
            }
            if (judgefish10 == true)
            {
                FishDecision.MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(0, 0, 0), 0);
            }
        }
        #endregion

        #region 左半场下半场复位
        public static void LeftRestart(Mission mission, ref Decision decision, int teamId, int fishId, bool judgefish04, bool judgefish05, bool judgefish06,
    bool judgefish01, bool judgefish02, bool judgefish03, bool judgefish07, bool judgefish08, bool judgefish09, bool judgefish10, xna.Vector3 fish)
        {
            if (judgefish04 == true)
            {
                FishDecision.MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(0, 0, 0), 0);
            }
            if (judgefish05 == true)
            {
                if (fish.Z < 0)
                {
                    FishDecision.MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(1300, 0, -700), 0);
                }
                else
                {
                    FishDecision.MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(1300, 0, 700), 0);
                }
            }
            if (judgefish06 == true)
            {
                FishDecision.MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(0, 0, 0), 0);
            }
            if (judgefish01 == true)
            {
                FishDecision.MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(0, 0, 0), 0);
            }
            if (judgefish02 == true)
            {
                if (fish.Z < 0)
                {
                    FishDecision.MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-1300, 0, -700), 0);
                }
                else
                {
                    FishDecision.MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-1300, 0, 700), 0);
                }
            }
            if (judgefish03 == true)
            {
                FishDecision.MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(0, 0, 0), 0);
            }
            if (judgefish07 == true)
            {
                FishDecision.MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(0, 0, 0), 0);
            }
            if (judgefish08 == true)
            {
                FishDecision.MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(0, 0, 0), 0);
            }
            if (judgefish09 == true)
            {
                FishDecision.MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(0, 0, 0), 0);
            }
            if (judgefish10 == true)
            {
                FishDecision.MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(0, 0, 0), 0);
            }
        }
        #endregion

        #region 判断进球分数
        public static int mark(int b0, int b1, int b2, int b3, int b4, int b5, int b6, int b7, int b8)
        {
            int a = 0, b = 0, c = 0, d = 0, e = 0, f = 0, g = 0, h = 0, i = 0, j = 0;
            if (b0 == 1)
            {
                a = 3;
            }
            if (b1 == 1)
            {
                b = 3;
            }
            if (b2 == 1)
            {
                c = 3;
            }
            if (b3 == 1)
            {
                d = 1;
            }
            if (b4 == 1)
            {
                e = 1;
            }
            if (b5 == 1)
            {
                f = 1;
            }
            if (b6 == 1)
            {
                g = 1;
            }
            if (b7 == 1)
            {
                h = 2;
            }
            if (b8 == 1)
            {
                i = 2;
            }
            j = a + b + c + d + e + f + g + h + i;
            return j;
        }
        #endregion

        #region New2v2SweepBall
        /// <summary>
        /// 让鱼在四个角上扫球，让球进入球门范围内，方便进球
        /// </summary>
        /// <param name="mission"></param>
        /// <param name="decision"></param>
        /// <param name="teamId"></param>
        /// <param name="fishId"></param>
        /// <param name="flag">左上-1；左下-3；右上-4；右下-6</param>
        public static void SweepBall(Mission mission, ref Decision decision, int teamId, int fishId, int flag)
        {
            //一些关键量
            Vector3 fishBody = mission.TeamsRef[teamId].Fishes[fishId].PositionMm;
            Vector3 fishHead = mission.TeamsRef[teamId].Fishes[fishId].PositionMm;

            if (flag == 1)
            {
                if (JudgePIsInArea(fishBody, 2))
                {
                    if (fishHead.X <= -1300f && Math.Abs(fishHead.Z) <= 200)
                    {
                        Dribble2(ref decision, mission.TeamsRef[teamId].Fishes[fishId], SetDegreeFishToP(fishBody, -135), 7, 4, 1, 0, 10, 10, 10, 10, 100, true);
                    }
                    else
                    {
                        Dribble2(ref decision, mission.TeamsRef[teamId].Fishes[fishId], SetDegreeFishToP(fishBody, +135), 7, 4, 1, 0, 10, 10, 10, 10, 100, true);
                    }
                }

                if (JudgePIsInArea(fishBody, 3))
                {
                    Vector3 temp = new Vector3(-660f, 0f, 760f);
                    MoveToAreaS(mission, ref decision, teamId, fishId, temp, 0);
                }

                if (JudgePIsInArea(fishBody, 5) && fishBody.Z >= 0)
                {
                    Vector3 temp = new Vector3(1275f, 0f, 725f);
                    MoveToAreaS(mission, ref decision, teamId, fishId, temp, +90);
                }

                if (JudgePIsInArea(fishBody, 5) && fishBody.Z < 0)
                {
                    Vector3 temp = new Vector3(1275f, 0f, -725f);
                    MoveToAreaS(mission, ref decision, teamId, fishId, temp, -90);
                }

                if (JudgePIsInArea(fishBody, 6))
                {
                    Vector3 temp = new Vector3(0f, 0f, 700f);
                    MoveToAreaS(mission, ref decision, teamId, fishId, temp, -180);
                }

                if (JudgePIsInArea(fishBody, 4))
                {
                    Vector3 temp = new Vector3(0f, 0f, -700f);
                    MoveToAreaS(mission, ref decision, teamId, fishId, temp, -180);
                }

                if (JudgePIsInArea(fishBody, 10))
                {
                    Vector3 temp = new Vector3(-580f, 0f, 340f);
                    MoveToAreaS(mission, ref decision, teamId, fishId, temp, -90);
                }

                if (JudgePIsInArea(fishBody, 9))
                {
                    Vector3 temp = new Vector3(-580f, 0f, -340f);
                    MoveToAreaS(mission, ref decision, teamId, fishId, temp, -90);
                }

                if (JudgePIsInArea(fishBody, 8))
                {
                    Vector3 temp = new Vector3(-660f, 0f, -760f);
                    MoveToAreaS(mission, ref decision, teamId, fishId, temp, -90);
                }

                if (JudgePIsInArea(fishBody, 7))
                {
                    Vector3 temp = new Vector3(-1080f, 0f, -980f);
                    MoveToAreaS(mission, ref decision, teamId, fishId, temp, -150);
                }

                if (JudgePIsInArea(fishHead, 1))
                {
                    if (fishHead.X <= -1300f && fishHead.Z <= -800f)
                    {
                        Vector3 temp = new Vector3(-1425f, 0f, -600f);
                        FishTrun(mission, ref decision, teamId, fishId, temp);
                    }
                    else if (fishHead.X <= -1300f && fishHead.Z > -800f)
                    {
                        Vector3 _temp = SetDegreeFishToP(fishBody, +135);
                        Dribble2(ref decision, mission.TeamsRef[teamId].Fishes[fishId], _temp, 7, 4, 1, 0, 7, 7, 7, 10, 100, true);
                    }
                    else
                    {
                        Vector3 temp = SetDegreeFishToP(fishBody, -135);
                        Dribble2(ref decision, mission.TeamsRef[teamId].Fishes[fishId], temp, 7, 4, 1, 0, 9, 9, 9, 10, 100, true);
                    }
                }
            }

            if (flag == 3)
            {
                if (JudgePIsInArea(fishBody, 2))
                {
                    if (fishHead.X <= -1300f && Math.Abs(fishHead.Z) <= 200)
                    {
                        Dribble2(ref decision, mission.TeamsRef[teamId].Fishes[fishId], SetDegreeFishToP(fishBody, +135), 7, 4, 1, 0, 10, 10, 10, 10, 100, true);
                    }
                    else
                    {
                        Dribble2(ref decision, mission.TeamsRef[teamId].Fishes[fishId], SetDegreeFishToP(fishBody, -135), 7, 4, 1, 0, 10, 10, 10, 10, 100, true);
                    }
                }

                if (JudgePIsInArea(fishBody, 1))
                {
                    Vector3 temp = new Vector3(-660f, 0f, -760f);
                    MoveToAreaS(mission, ref decision, teamId, fishId, temp, 0);
                }

                if (JudgePIsInArea(fishBody, 5) && fishBody.Z >= 0)
                {
                    Vector3 temp = new Vector3(1275f, 0f, 725f);
                    MoveToAreaS(mission, ref decision, teamId, fishId, temp, +90);
                }

                if (JudgePIsInArea(fishBody, 5) && fishBody.Z < 0)
                {
                    Vector3 temp = new Vector3(1275f, 0f, -725f);
                    MoveToAreaS(mission, ref decision, teamId, fishId, temp, -90);
                }

                if (JudgePIsInArea(fishBody, 6))
                {
                    Vector3 temp = new Vector3(0f, 0f, 700f);
                    MoveToAreaS(mission, ref decision, teamId, fishId, temp, -180);
                }

                if (JudgePIsInArea(fishBody, 4))
                {
                    Vector3 temp = new Vector3(0f, 0f, -700f);
                    MoveToAreaS(mission, ref decision, teamId, fishId, temp, -180);
                }

                if (JudgePIsInArea(fishBody, 10))
                {
                    Vector3 temp = new Vector3(-1080f, 0f, 980f);
                    MoveToAreaS(mission, ref decision, teamId, fishId, temp, -90);
                }

                if (JudgePIsInArea(fishBody, 9))
                {
                    Vector3 temp = new Vector3(-660f, 0f, 760f);
                    MoveToAreaS(mission, ref decision, teamId, fishId, temp, -90);
                }

                if (JudgePIsInArea(fishBody, 8))
                {
                    Vector3 temp = new Vector3(-580f, 0f, 340f);
                    MoveToAreaS(mission, ref decision, teamId, fishId, temp, -90);
                }

                if (JudgePIsInArea(fishBody, 7))
                {
                    Vector3 temp = new Vector3(-580f, 0f, -340f);
                    MoveToAreaS(mission, ref decision, teamId, fishId, temp, -150);
                }

                if (JudgePIsInArea(fishHead, 3))
                {
                    if (fishHead.X <= -1300f && fishHead.Z >= 800f)
                    {
                        Vector3 temp = new Vector3(-1425f, 0f, 600f);
                        FishTrun(mission, ref decision, teamId, fishId, temp);
                    }
                    else if (fishHead.X < -1300f && fishHead.Z < 800f)
                    {
                        Vector3 _temp = SetDegreeFishToP(fishBody, -135);
                        Dribble2(ref decision, mission.TeamsRef[teamId].Fishes[fishId], _temp, 7, 4, 1, 0, 7, 7, 7, 10, 100, true);
                    }
                    else
                    {
                        Vector3 temp = SetDegreeFishToP(fishBody, +135);
                        Dribble2(ref decision, mission.TeamsRef[teamId].Fishes[fishId], temp, 7, 4, 1, 0, 9, 9, 9, 10, 100, true);
                    }
                }
            }

            if (flag == 4)
            {
                if (JudgePIsInArea(fishBody, 5))
                {
                    if (fishHead.X >= 1300f && Math.Abs(fishHead.Z) <= 200)
                    {
                        Dribble2(ref decision, mission.TeamsRef[teamId].Fishes[fishId], SetDegreeFishToP(fishBody, +45), 7, 4, 1, 0, 10, 10, 10, 10, 100, true);
                    }
                    else
                    {
                        Dribble2(ref decision, mission.TeamsRef[teamId].Fishes[fishId], SetDegreeFishToP(fishBody, -45), 7, 4, 1, 0, 10, 10, 10, 10, 100, true);
                    }
                }

                if (JudgePIsInArea(fishBody, 3))
                {
                    Vector3 temp = new Vector3(660f, 0f, 760f);
                    MoveToAreaS(mission, ref decision, teamId, fishId, temp, 0);
                }

                if (JudgePIsInArea(fishBody, 2) && fishBody.Z >= 0)
                {
                    Vector3 temp = new Vector3(-1275f, 0f, 725f);
                    MoveToAreaS(mission, ref decision, teamId, fishId, temp, +90);
                }

                if (JudgePIsInArea(fishBody, 2) && fishBody.Z < 0)
                {
                    Vector3 temp = new Vector3(-1275f, 0f, -725f);
                    MoveToAreaS(mission, ref decision, teamId, fishId, temp, -90);
                }

                if (JudgePIsInArea(fishBody, 6))
                {
                    Vector3 temp = new Vector3(0f, 0f, 700f);
                    MoveToAreaS(mission, ref decision, teamId, fishId, temp, -180);
                }

                if (JudgePIsInArea(fishBody, 1))
                {
                    Vector3 temp = new Vector3(0f, 0f, -700f);
                    MoveToAreaS(mission, ref decision, teamId, fishId, temp, -180);
                }

                if (JudgePIsInArea(fishBody, 10))
                {
                    Vector3 temp = new Vector3(580f, 0f, 340f);
                    MoveToAreaS(mission, ref decision, teamId, fishId, temp, -90);
                }

                if (JudgePIsInArea(fishBody, 9))
                {
                    Vector3 temp = new Vector3(580f, 0f, -340f);
                    MoveToAreaS(mission, ref decision, teamId, fishId, temp, -90);
                }

                if (JudgePIsInArea(fishBody, 8))
                {
                    Vector3 temp = new Vector3(660f, 0f, -760f);
                    MoveToAreaS(mission, ref decision, teamId, fishId, temp, -90);
                }

                if (JudgePIsInArea(fishBody, 7))
                {
                    Vector3 temp = new Vector3(1080f, 0f, -980f);
                    MoveToAreaS(mission, ref decision, teamId, fishId, temp, -30);
                }

                if (JudgePIsInArea(fishHead, 4))
                {
                    if (fishHead.X >= 1300f && fishHead.Z <= -800f)
                    {
                        Vector3 temp = new Vector3(1425f, 0f, -600f);
                        FishTrun(mission, ref decision, teamId, fishId, temp);
                    }
                    else if (fishHead.X >= 1300f && fishHead.Z > -800f)
                    {
                        Vector3 _temp = SetDegreeFishToP(fishBody, +45);
                        Dribble2(ref decision, mission.TeamsRef[teamId].Fishes[fishId], _temp, 7, 4, 1, 0, 7, 7, 7, 10, 100, true);
                    }
                    else
                    {
                        Vector3 temp = SetDegreeFishToP(fishBody, -45);
                        Dribble2(ref decision, mission.TeamsRef[teamId].Fishes[fishId], temp, 7, 4, 1, 0, 9, 9, 9, 10, 100, true);
                    }
                }
            }

            if (flag == 6)
            {
                if (JudgePIsInArea(fishBody, 5))
                {
                    if (fishHead.X >= 1300f && Math.Abs(fishHead.Z) <= 200)
                    {
                        Dribble2(ref decision, mission.TeamsRef[teamId].Fishes[fishId], SetDegreeFishToP(fishBody, -45), 7, 4, 1, 0, 10, 10, 10, 10, 100, true);
                    }
                    else
                    {
                        Dribble2(ref decision, mission.TeamsRef[teamId].Fishes[fishId], SetDegreeFishToP(fishBody, +45), 7, 4, 1, 0, 10, 10, 10, 10, 100, true);
                    }
                }

                if (JudgePIsInArea(fishBody, 3))
                {
                    Vector3 temp = new Vector3(660f, 0f, 760f);
                    MoveToAreaS(mission, ref decision, teamId, fishId, temp, 0);
                }

                if (JudgePIsInArea(fishBody, 2) && fishBody.Z >= 0)
                {
                    Vector3 temp = new Vector3(-1275f, 0f, 725f);
                    MoveToAreaS(mission, ref decision, teamId, fishId, temp, +90);
                }

                if (JudgePIsInArea(fishBody, 2) && fishBody.Z < 0)
                {
                    Vector3 temp = new Vector3(-1275f, 0f, -725f);
                    MoveToAreaS(mission, ref decision, teamId, fishId, temp, -90);
                }

                if (JudgePIsInArea(fishBody, 4))
                {
                    Vector3 temp = new Vector3(0f, 0f, -700f);
                    MoveToAreaS(mission, ref decision, teamId, fishId, temp, -180);
                }

                if (JudgePIsInArea(fishBody, 1))
                {
                    Vector3 temp = new Vector3(0f, 0f, -700f);
                    MoveToAreaS(mission, ref decision, teamId, fishId, temp, -180);
                }

                if (JudgePIsInArea(fishBody, 7))
                {
                    Vector3 temp = new Vector3(580f, 0f, -340f);
                    MoveToAreaS(mission, ref decision, teamId, fishId, temp, +90);
                }

                if (JudgePIsInArea(fishBody, 8))
                {
                    Vector3 temp = new Vector3(580f, 0f, 340f);
                    MoveToAreaS(mission, ref decision, teamId, fishId, temp, +90);
                }

                if (JudgePIsInArea(fishBody, 9))
                {
                    Vector3 temp = new Vector3(660f, 0f, 760f);
                    MoveToAreaS(mission, ref decision, teamId, fishId, temp, +90);
                }

                if (JudgePIsInArea(fishBody, 10))
                {
                    Vector3 temp = new Vector3(1080f, 0f, 980f);
                    MoveToAreaS(mission, ref decision, teamId, fishId, temp, +30);
                }

                if (JudgePIsInArea(fishHead, 6))
                {
                    if (fishHead.X >= 1300f && fishHead.Z >= 800f)
                    {
                        Vector3 temp = new Vector3(1425f, 0f, 600f);
                        FishTrun(mission, ref decision, teamId, fishId, temp);
                    }
                    else if (fishHead.X >= 1300f && fishHead.Z < 800f)
                    {
                        Vector3 _temp = SetDegreeFishToP(fishBody, -45);
                        Dribble2(ref decision, mission.TeamsRef[teamId].Fishes[fishId], _temp, 7, 4, 1, 0, 7, 7, 7, 10, 100, true);
                    }
                    else
                    {
                        Vector3 temp = SetDegreeFishToP(fishBody, +45);
                        Dribble2(ref decision, mission.TeamsRef[teamId].Fishes[fishId], temp, 7, 4, 1, 0, 9, 9, 9, 10, 100, true);
                    }
                }
            }
        }


        #endregion

        #region New2v2Disturb
        /// <summary>
        /// 捣乱算法
        /// </summary>
        /// <param name="mission"></param>
        /// <param name="decision"></param>
        /// <param name="teamId"></param>
        /// <param name="fishId"></param>
        /// <param name="enemyDoor">敌方球门位置，左侧为0，右侧为1</param>
        public static void Disturb(Mission mission, ref Decision decision, int teamId, int fishId, int enemyDoor)
        {
            //一些关键量
            Vector3 fishBody = mission.TeamsRef[teamId].Fishes[fishId].PositionMm;
            Vector3 fishHead = mission.TeamsRef[teamId].Fishes[fishId].PolygonVertices[0];
            float fishRad = mission.TeamsRef[teamId].Fishes[fishId].BodyDirectionRad;

            Vector3[] ball = new Vector3[9];
            ball[0] = mission.EnvRef.Balls[0].PositionMm;
            ball[1] = mission.EnvRef.Balls[1].PositionMm;
            ball[2] = mission.EnvRef.Balls[2].PositionMm;
            ball[3] = mission.EnvRef.Balls[3].PositionMm;
            ball[4] = mission.EnvRef.Balls[4].PositionMm;
            ball[5] = mission.EnvRef.Balls[5].PositionMm;
            ball[6] = mission.EnvRef.Balls[6].PositionMm;
            ball[7] = mission.EnvRef.Balls[7].PositionMm;
            ball[8] = mission.EnvRef.Balls[8].PositionMm;

            float A = fishBody.Z - fishHead.Z;
            float B = fishHead.X - fishBody.X;
            float C = fishBody.X * fishHead.Z - fishBody.Z * fishHead.X;


            if (enemyDoor == 1)
            {
                //如果最近的鱼在区域8，快速移动到区域7内
                int isFishIn8 = 0;
                if (JudgePIsInArea(fishHead, 8))
                {
                    isFishIn8 = 1;
                }
                if (isFishIn8 == 1)
                {
                    Vector3 temp = new Vector3(770f, 0f, -770f);
                    MoveToAreaS(mission, ref decision, teamId, fishId, temp, 0);
                }

                //如果最近的鱼在区域9，快速移动到区域10内
                int isFishIn9 = 0;
                if (JudgePIsInArea(fishHead, 9))
                {
                    isFishIn9 = 1;
                }
                if (isFishIn9 == 1)
                {
                    Vector3 temp = new Vector3(770f, 0f, 770f);
                    MoveToAreaS(mission, ref decision, teamId, fishId, temp, 0);
                }










                //如果最近的鱼在区域7或4，前进到对方球门上方
                int isFishIn7Or4 = 0;
                if (JudgePIsInArea(fishHead, 7) || JudgePIsInArea(fishHead, 4))
                {
                    isFishIn7Or4 = 1;
                }
                if (isFishIn7Or4 == 1)
                {
                    Vector3 temp = new Vector3(1200f, 0f, -640f);
                    MoveToAreaS(mission, ref decision, teamId, fishId, temp, 0);
                }

                //如果最近的鱼在区域10或6，前进到对方球门下方
                int isFishIn10Or6 = 0;
                if (JudgePIsInArea(fishHead, 10) || JudgePIsInArea(fishHead, 6))
                {
                    isFishIn10Or6 = 1;
                }
                if (isFishIn10Or6 == 1)
                {
                    Vector3 temp = new Vector3(1200f, 0f, 640f);
                    MoveToAreaS(mission, ref decision, teamId, fishId, temp, 0);
                }










                //如果鱼体中心到达目标位置，转到合适角度，斜插进球门(上方)
                int isCloseToTargetUp = 0;
                Vector3 importantPtUp = new Vector3(1250f, 0f, -600f);
                if (Math.Pow(fishBody.X - importantPtUp.X, 2.0) + Math.Pow(fishBody.Z - importantPtUp.Z, 2.0) < Math.Pow(100, 2.0))
                {
                    isCloseToTargetUp = 1;
                }
                if (isCloseToTargetUp == 1)
                {
                    Vector3 temp = new Vector3(1000f, 0f, -365f);
                    FishTrun(mission, ref decision, teamId, fishId, temp);
                }

                //如果鱼体中心到达目标位置，转到合适角度，斜插进球门(下方)
                int isCloseToTargetDown = 0;
                Vector3 importantPtDown = new Vector3(1250f, 0f, 600f);
                if (Math.Pow(fishBody.X - importantPtDown.X, 2.0) + Math.Pow(fishBody.Z - importantPtDown.Z, 2.0) < Math.Pow(100, 2.0))
                {
                    isCloseToTargetDown = 1;
                }
                if (isCloseToTargetDown == 1)
                {
                    Vector3 temp = new Vector3(1000f, 0f, 365f);
                    FishTrun(mission, ref decision, teamId, fishId, temp);
                }








                //如果鱼头进入球门，根据球门内球的分布来决定扫门方向
                int ballUp = 0;
                if (JudgePIsInArea(ball[0], 5) && A * ball[0].X + B * ball[0].Z < C)
                    ballUp++;
                if (JudgePIsInArea(ball[1], 5) && A * ball[1].X + B * ball[1].Z < C)
                    ballUp++;
                if (JudgePIsInArea(ball[2], 5) && A * ball[2].X + B * ball[2].Z < C)
                    ballUp++;
                if (JudgePIsInArea(ball[3], 5) && A * ball[3].X + B * ball[3].Z < C)
                    ballUp++;
                if (JudgePIsInArea(ball[4], 5) && A * ball[4].X + B * ball[4].Z < C)
                    ballUp++;
                if (JudgePIsInArea(ball[5], 5) && A * ball[5].X + B * ball[5].Z < C)
                    ballUp++;
                if (JudgePIsInArea(ball[6], 5) && A * ball[6].X + B * ball[6].Z < C)
                    ballUp++;
                if (JudgePIsInArea(ball[7], 5) && A * ball[7].X + B * ball[7].Z < C)
                    ballUp++;
                if (JudgePIsInArea(ball[8], 5) && A * ball[8].X + B * ball[8].Z < C)
                    ballUp++;
                int ballDown = 0;
                if (JudgePIsInArea(ball[0], 5) && A * ball[0].X + B * ball[0].Z >= C)
                    ballDown++;
                if (JudgePIsInArea(ball[1], 5) && A * ball[1].X + B * ball[1].Z >= C)
                    ballDown++;
                if (JudgePIsInArea(ball[2], 5) && A * ball[2].X + B * ball[2].Z >= C)
                    ballDown++;
                if (JudgePIsInArea(ball[3], 5) && A * ball[3].X + B * ball[3].Z >= C)
                    ballDown++;
                if (JudgePIsInArea(ball[4], 5) && A * ball[4].X + B * ball[4].Z >= C)
                    ballDown++;
                if (JudgePIsInArea(ball[5], 5) && A * ball[5].X + B * ball[5].Z >= C)
                    ballDown++;
                if (JudgePIsInArea(ball[6], 5) && A * ball[6].X + B * ball[6].Z >= C)
                    ballDown++;
                if (JudgePIsInArea(ball[7], 5) && A * ball[7].X + B * ball[7].Z >= C)
                    ballDown++;
                if (JudgePIsInArea(ball[8], 5) && A * ball[8].X + B * ball[8].Z >= C)
                    ballDown++;
                if (JudgePIsInArea(fishHead, 5))
                {
                    if (ballUp >= ballDown)
                    {
                        Vector3 destiny = SetDegreeFishToP(fishBody, -135);
                        Dribble(ref decision, mission.TeamsRef[teamId].Fishes[fishId], destiny, GetFishToDestRad(fishBody, destiny, fishRad), (float)(Math.PI / 2), (float)(Math.PI / 4), 0, 10, 10, 10, 100, true);
                    }
                    else
                    {
                        Vector3 destiny = SetDegreeFishToP(fishBody, +135);
                        Dribble(ref decision, mission.TeamsRef[teamId].Fishes[fishId], destiny, GetFishToDestRad(fishBody, destiny, fishRad), (float)(Math.PI / 2), (float)(Math.PI / 4), 0, 10, 10, 10, 100, true);
                    }
                }
            }
            else
            {
                //如果最近的鱼在区域8，快速移动到区域7内
                int isFishIn8 = 0;
                if (JudgePIsInArea(fishHead, 8))
                {
                    isFishIn8 = 1;
                }
                if (isFishIn8 == 1)
                {
                    Vector3 temp = new Vector3(-770f, 0f, -770f);
                    MoveToAreaS(mission, ref decision, teamId, fishId, temp, 0);
                }

                //如果最近的鱼在区域9，快速移动到区域10内
                int isFishIn9 = 0;
                if (JudgePIsInArea(fishHead, 9))
                {
                    isFishIn9 = 1;
                }
                if (isFishIn9 == 1)
                {
                    Vector3 temp = new Vector3(-770f, 0f, 770f);
                    MoveToAreaS(mission, ref decision, teamId, fishId, temp, 0);
                }










                //如果最近的鱼在区域7或1，前进到对方球门上方
                int isFishIn7Or1 = 0;
                if (JudgePIsInArea(fishHead, 7) || JudgePIsInArea(fishHead, 1))
                {
                    isFishIn7Or1 = 1;
                }
                if (isFishIn7Or1 == 1)
                {
                    Vector3 temp = new Vector3(-1200f, 0f, -640f);
                    MoveToAreaS(mission, ref decision, teamId, fishId, temp, 0);
                }

                //如果最近的鱼在区域10或3，前进到对方球门下方
                int isFishIn10Or3 = 0;
                if (JudgePIsInArea(fishHead, 10) || JudgePIsInArea(fishHead, 6))
                {
                    isFishIn10Or3 = 1;
                }
                if (isFishIn10Or3 == 1)
                {
                    Vector3 temp = new Vector3(-1200f, 0f, 640f);
                    MoveToAreaS(mission, ref decision, teamId, fishId, temp, 0);
                }










                //如果鱼体中心到达目标位置，转到合适角度，斜插进球门(上方)
                int isCloseToTargetUp = 0;
                Vector3 importantPtUp = new Vector3(-1250f, 0f, -600f);
                if (Math.Pow(fishBody.X - importantPtUp.X, 2.0) + Math.Pow(fishBody.Z - importantPtUp.Z, 2.0) < Math.Pow(100, 2.0))
                {
                    isCloseToTargetUp = 1;
                }
                if (isCloseToTargetUp == 1)
                {
                    Vector3 temp = new Vector3(-1000f, 0f, -365f);
                    FishTrun(mission, ref decision, teamId, fishId, temp);
                }

                //如果鱼体中心到达目标位置，转到合适角度，斜插进球门(下方)
                int isCloseToTargetDown = 0;
                Vector3 importantPtDown = new Vector3(-1250f, 0f, 600f);
                if (Math.Pow(fishBody.X - importantPtDown.X, 2.0) + Math.Pow(fishBody.Z - importantPtDown.Z, 2.0) < Math.Pow(100, 2.0))
                {
                    isCloseToTargetDown = 1;
                }
                if (isCloseToTargetDown == 1)
                {
                    Vector3 temp = new Vector3(-1000f, 0f, 365f);
                    FishTrun(mission, ref decision, teamId, fishId, temp);
                }








                //如果鱼头进入球门，根据球门内球的分布来决定扫门方向
                int ballUp = 0;
                if (JudgePIsInArea(ball[0], 5) && A * ball[0].X + B * ball[0].Z < C)
                    ballUp++;
                if (JudgePIsInArea(ball[1], 5) && A * ball[1].X + B * ball[1].Z < C)
                    ballUp++;
                if (JudgePIsInArea(ball[2], 5) && A * ball[2].X + B * ball[2].Z < C)
                    ballUp++;
                if (JudgePIsInArea(ball[3], 5) && A * ball[3].X + B * ball[3].Z < C)
                    ballUp++;
                if (JudgePIsInArea(ball[4], 5) && A * ball[4].X + B * ball[4].Z < C)
                    ballUp++;
                if (JudgePIsInArea(ball[5], 5) && A * ball[5].X + B * ball[5].Z < C)
                    ballUp++;
                if (JudgePIsInArea(ball[6], 5) && A * ball[6].X + B * ball[6].Z < C)
                    ballUp++;
                if (JudgePIsInArea(ball[7], 5) && A * ball[7].X + B * ball[7].Z < C)
                    ballUp++;
                if (JudgePIsInArea(ball[8], 5) && A * ball[8].X + B * ball[8].Z < C)
                    ballUp++;
                int ballDown = 0;
                if (JudgePIsInArea(ball[0], 5) && A * ball[0].X + B * ball[0].Z >= C)
                    ballDown++;
                if (JudgePIsInArea(ball[1], 5) && A * ball[1].X + B * ball[1].Z >= C)
                    ballDown++;
                if (JudgePIsInArea(ball[2], 5) && A * ball[2].X + B * ball[2].Z >= C)
                    ballDown++;
                if (JudgePIsInArea(ball[3], 5) && A * ball[3].X + B * ball[3].Z >= C)
                    ballDown++;
                if (JudgePIsInArea(ball[4], 5) && A * ball[4].X + B * ball[4].Z >= C)
                    ballDown++;
                if (JudgePIsInArea(ball[5], 5) && A * ball[5].X + B * ball[5].Z >= C)
                    ballDown++;
                if (JudgePIsInArea(ball[6], 5) && A * ball[6].X + B * ball[6].Z >= C)
                    ballDown++;
                if (JudgePIsInArea(ball[7], 5) && A * ball[7].X + B * ball[7].Z >= C)
                    ballDown++;
                if (JudgePIsInArea(ball[8], 5) && A * ball[8].X + B * ball[8].Z >= C)
                    ballDown++;
                if (JudgePIsInArea(fishHead, 2))
                {
                    if (ballUp >= ballDown)
                    {
                        Vector3 destiny = SetDegreeFishToP(fishBody, +45);
                        Dribble(ref decision, mission.TeamsRef[teamId].Fishes[fishId], destiny, GetFishToDestRad(fishBody, destiny, fishRad), (float)(Math.PI / 2), (float)(Math.PI / 4), 0, 10, 10, 10, 100, true);
                    }
                    else
                    {
                        Vector3 destiny = SetDegreeFishToP(fishBody, -45);
                        Dribble(ref decision, mission.TeamsRef[teamId].Fishes[fishId], destiny, GetFishToDestRad(fishBody, destiny, fishRad), (float)(Math.PI / 2), (float)(Math.PI / 4), 0, 10, 10, 10, 100, true);
                    }
                }
            }
        }

        #endregion

        #region !进球!
        /// <param name="area"> 1-左边 2-右边</param>
        public static void Goal(int area, Mission mission, ref Decision decision, int teamId, int fishId, int ballId)
        {
            Vector3 fishBody = mission.TeamsRef[teamId].Fishes[fishId].PositionMm;
            Vector3 fishHead = mission.TeamsRef[teamId].Fishes[fishId].PolygonVertices[0];
            int fisharea = FishArea(fishBody);
            Vector3 ball = mission.EnvRef.Balls[ballId].PositionMm;
            int ballarea = FishArea(ball);
            if (area == 1)
            {
                if (ballarea == 1)
                {
                    if (fisharea == 1)
                    {
                        Dribble_Will1( mission,ref decision,teamId,fishId,ballId, new Vector3(-1350, 0, -450));
                    }
                    if (fisharea == 2)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-1150, 0, -670), 0);
                    }
                    if (fisharea == 3)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-1300, 0, -540), 0);
                    }
                    if (fisharea == 4 || fisharea == 6 || fisharea == 7 || fisharea == 8 || fisharea == 9 || fisharea == 10)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, ball, -180);
                    }
                    if (fisharea == 5)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(1450, 0, -800), 0);
                    }
                }

                if (ballarea == 2)
                {
                    if (fisharea == 1)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-1250, 0, -490), 0);
                    }
                    if (fisharea == 2)
                    {
                        Dribble3(ref decision, mission.TeamsRef[teamId].Fishes[fishId], new Vector3(-1040, 0, ball.Z), ball);
                    }
                    if (fisharea == 3)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-1250, 0, 490), 0);
                    }
                    if (fisharea == 4 || fisharea == 7 || fisharea == 8)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-1050, 0, -950), 0);
                    }
                    if (fisharea == 6 || fisharea == 9 || fisharea == 10)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-1050, 0, 950), 0);
                    }
                    if (fisharea == 5)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(1450, 0, -800), 0);
                    }
                }

                if (ballarea == 3)
                {
                    if (fisharea == 3)
                    {
                        Dribble_Will1(mission, ref decision, teamId, fishId, ballId, new Vector3(-1350, 0, 450));
                    }
                    if (fisharea == 2)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-1450, 0, 600), 0);
                    }
                    if (fisharea == 1)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-1450, 0, 600), 0);
                    }
                    if (fisharea == 4 || fisharea == 6 || fisharea == 7 || fisharea == 8 || fisharea == 9 || fisharea == 10)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-1000, 0, 950), 0);
                    }
                    if (fisharea == 5)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(1450, 0, 800), 0);
                    }
                }
                
                if (ballarea == 7)
                {
                    if (fisharea == 1)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-700, 0, -900), 0);
                    }
                    if (fisharea == 2)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-1450, 0, -600), 0);
                    }
                    if (fisharea == 3)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-700, 0, 900), 0);
                    }
                    if (fisharea == 4 || fisharea == 8 || fisharea == 9 || fisharea == 10)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, ball, 0);
                    }
                    if (fisharea == 7)
                    {
                        Dribble_Will1(mission, ref decision, teamId, fishId, ballId, new Vector3(-1050, 0, -960));
                    }
                    if (fisharea == 5)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(1450, 0, -600), 0);
                    }
                    if (fisharea == 6)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(700, 0, 900), 0);
                    }
                }

                if (ballarea == 8)
                {
                    if (fisharea == 1)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-700, 0, -900), 0);
                    }
                    if (fisharea == 2)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-1450, 0, -600), 0);
                    }
                    if (fisharea == 3)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-700, 0, 900), 0);
                    }
                    if (fisharea == 4)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(700, 0, -900), 0);
                    }
                    if (fisharea == 5)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(1450, 0, -600), 0);
                    }
                    if (fisharea == 6)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(700, 0, 900), 0);
                    }
                    if (fisharea == 7 || fisharea == 9 || fisharea == 10)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, ball, 0);
                    }
                    if (fisharea == 8)
                    {
                        Dribble_Will1(mission, ref decision, teamId, fishId, ballId, new Vector3(-1000, 0, -700));
                    }
                }

                if (ballarea == 9)
                {
                    if (fisharea == 1)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-700, 0, -900), 0);
                    }
                    if (fisharea == 2)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-1450, 0, 600), 0);
                    }
                    if (fisharea == 3)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-700, 0, 900), 0);
                    }
                    if (fisharea == 4)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(700, 0, -900), 0);
                    }
                    if (fisharea == 5)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(1450, 0, 600), 0);
                    }
                    if (fisharea == 6)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(700, 0, 900), 0);
                    }
                    if (fisharea == 7 || fisharea == 8 || fisharea == 10)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, ball, 0);
                    }
                    if (fisharea == 9)
                    {
                        Dribble_Will1(mission, ref decision, teamId, fishId, ballId, new Vector3(-1050, 0, 700));
                    }
                }

                if (ballarea == 10)
                {
                    if (fisharea == 1)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-700, 0, -900), 0);
                    }
                    if (fisharea == 2)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-1450, 0, 600), 0);
                    }
                    if (fisharea == 3)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-700, 0, 900), 0);
                    }
                    if (fisharea == 4)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(700, 0, -900), 0);
                    }
                    if (fisharea == 5)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(1450, 0, 600), 0);
                    }
                    if (fisharea == 6)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(700, 0, 900), 0);
                    }
                    if (fisharea == 7 || fisharea == 8 || fisharea == 9)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, ball, 0);
                    }
                    if (fisharea == 10)
                    {
                        Dribble3(ref decision, mission.TeamsRef[teamId].Fishes[fishId], new Vector3(-1050, 0, 1000), ball);
                    }
                }

                if (ballarea == 4)
                {
                    if (fisharea == 1)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-700, 0, -900), 0);
                    }
                    if (fisharea == 2)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-1450, 0, -600), 0);
                    }
                    if (fisharea == 3)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-700, 0, 900), 0);
                    }
                    if (fisharea == 4)
                    {
                        Dribble3(ref decision, mission.TeamsRef[teamId].Fishes[fishId], new Vector3(-1000, 0, -700), ball);
                    }
                    if (fisharea == 5)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(1450, 0, -600), 0);
                    }
                    if (fisharea == 6)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(1450, 0, 450), 0);
                    }
                    if (fisharea == 7 || fisharea == 8 || fisharea == 9 || fisharea == 10)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(1000, 0, -950), (int)Math.PI);
                    }
                }

                if (ballarea == 5)
                {
                    if (fisharea == 1)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-700, 0, -900), 0);
                    }
                    if (fisharea == 2)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-1450, 0, -600), 0);
                    }
                    if (fisharea == 3)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-700, 0, 900), 0);
                    }
                    if (fisharea == 4)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(1250, 0, -512), -180);
                    }
                    if (fisharea == 6)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(1250, 0, 512), -180);
                    }
                    if (fisharea == 7 || fisharea == 8)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(1000, 0, -950), -180);
                    }
                    if (fisharea == 9 || fisharea == 10)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(1000, 0, 950), -180);
                    }
                    if (fisharea == 5)
                    {
                        Dribble3(ref decision, mission.TeamsRef[teamId].Fishes[fishId], new Vector3(1250, 0, -650), ball);
                    }
                }

                if (ballarea == 6)
                {
                    if (fisharea == 1)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-700, 0, -900), 0);
                    }
                    if (fisharea == 2)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-1450, 0, -600), 0);
                    }
                    if (fisharea == 3)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-700, 0, 900), 0);
                    }
                    if (fisharea == 6)
                    {
                        Dribble3(ref decision, mission.TeamsRef[teamId].Fishes[fishId], new Vector3(-1000, 0, 700), ball);
                    }
                    if (fisharea == 5)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(1450, 0, 600), 0);
                    }
                    if (fisharea == 4)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(1450, 0, 450), 0);
                    }
                    if (fisharea == 7 || fisharea == 8 || fisharea == 9 || fisharea == 10)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(1000, 0, 950), (int)Math.PI);
                    }
                }

            }

            else if (area == 2)
            {
                if (ballarea == 1)
                {
                    if (fisharea == 1)
                    {
                        Dribble3(ref decision, mission.TeamsRef[teamId].Fishes[fishId], new Vector3(-700, 0, -900), ball);
                    }
                    if (fisharea == 2)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-1450, 0, -600), 0);
                    }
                    if (fisharea == 3)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-1450, 0, -600), 0);
                    }
                    if (fisharea == 4 || fisharea == 6 || fisharea == 7 || fisharea == 8 || fisharea == 9 || fisharea == 10)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-1000, 0, -950), -180);
                    }
                    if (fisharea == 5)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(1450, 0, -800), -180);
                    }
                }

                if (ballarea == 2)
                {
                    if (fisharea == 1)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-1250, 0, -512), -180);
                    }
                    if (fisharea == 2)
                    {
                        Dribble3(ref decision, mission.TeamsRef[teamId].Fishes[fishId], new Vector3(-1400, 0, 650), ball);
                    }
                    if (fisharea == 3)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-1250, 0, 512), -180);
                    }
                    if (fisharea == 4 || fisharea == 7 || fisharea == 8)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-1000, 0, -950), -180);
                    }
                    if (fisharea == 6 || fisharea == 9 || fisharea == 10)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-1000, 0, 950), -180);
                    }
                    if (fisharea == 5)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(1450, 0, -800), -180);
                    }
                }

                if (ballarea == 3)
                {
                    if (fisharea == 3)
                    {
                        Dribble3(ref decision, mission.TeamsRef[teamId].Fishes[fishId], new Vector3(-700, 0, 900), ball);
                    }
                    if (fisharea == 2)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-1450, 0, 600), 0);
                    }
                    if (fisharea == 1)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-1450, 0, 600), 0);
                    }
                    if (fisharea == 4 || fisharea == 6 || fisharea == 7 || fisharea == 8 || fisharea == 9 || fisharea == 10)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-1000, 0, 950), -180);
                    }
                    if (fisharea == 5)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(1450, 0, 800), -180);
                    }
                }

                if (ballarea == 7)
                {
                    if (fisharea == 1)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, ball, 0);
                    }
                    if (fisharea == 2)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-1450, 0, -600), 0);
                    }
                    if (fisharea == 3)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-700, 0, 900), 0);
                    }
                    if (fisharea == 4 || fisharea == 8 || fisharea == 9 || fisharea == 10)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, ball, 0);
                    }
                    if (fisharea == 7)
                    {
                        Dribble_Will1(mission, ref decision, teamId, fishId, ballId, new Vector3(1000, 0, -960));
                    }
                    if (fisharea == 5)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(1450, 0, -600), 0);
                    }
                    if (fisharea == 6)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(700, 0, 900), 0);
                    }
                }

                if (ballarea == 8)
                {
                    if (fisharea == 1)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-700, 0, -900), 0);
                    }
                    if (fisharea == 2)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-1450, 0, -600), 0);
                    }
                    if (fisharea == 3)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-700, 0, 900), 0);
                    }
                    if (fisharea == 4)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(700, 0, -900), 0);
                    }
                    if (fisharea == 5)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(1450, 0, -600), 0);
                    }
                    if (fisharea == 6)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(700, 0, 900), 0);
                    }
                    if (fisharea == 7 || fisharea == 9 || fisharea == 10)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, ball, 0);
                    }
                    if (fisharea == 8)
                    {
                        Dribble_Will1(mission, ref decision, teamId, fishId, ballId, new Vector3(1000, 0, -700));
                    }
                }

                if (ballarea == 9)
                {
                    if (fisharea == 1)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-700, 0, -900), 0);
                    }
                    if (fisharea == 2)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-1450, 0, 600), 0);
                    }
                    if (fisharea == 3)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-700, 0, 900), 0);
                    }
                    if (fisharea == 4)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(700, 0, -900), 0);
                    }
                    if (fisharea == 5)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(1450, 0, 600), 0);
                    }
                    if (fisharea == 6)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(700, 0, 900), 0);
                    }
                    if (fisharea == 7 || fisharea == 8 || fisharea == 10)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, ball, 0);
                    }
                    if (fisharea == 9)
                    {
                        Dribble_Will1(mission, ref decision, teamId, fishId, ballId, new Vector3(1000, 0, 700));
                    }
                }

                if (ballarea == 10)
                {
                    if (fisharea == 1)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-700, 0, -900), 0);
                    }
                    if (fisharea == 2)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-1450, 0, 600), 0);
                    }
                    if (fisharea == 3)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-700, 0, 900), 0);
                    }
                    if (fisharea == 4)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(700, 0, -900), 0);
                    }
                    if (fisharea == 5)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(1450, 0, 600), 0);
                    }
                    if (fisharea == 6)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(700, 0, 900), 0);
                    }
                    if (fisharea == 7 || fisharea == 8 || fisharea == 9)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, ball, 0);
                    }
                    if (fisharea == 10)
                    {
                        Dribble3(ref decision, mission.TeamsRef[teamId].Fishes[fishId], new Vector3(1024, 0, 1000), ball);
                    }
                }

              /*  if (ballarea == 4)
                {
                    if (fisharea == 1)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-700, 0, -900), 0);
                    }
                    if (fisharea == 2)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-1450, 0, -600), 0);
                    }
                    if (fisharea == 3)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-700, 0, 900), 0);
                    }
                    if (fisharea == 4)
                    {
                        SweepBall(mission, ref decision, teamId, fishId, 4);
                    }
                    if (fisharea == 5)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(1150, 0, -670), 0);
                    }
                    if (fisharea == 6)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(1300, 0, 450), 0);
                    }
                    if (fisharea == 7 || fisharea == 8 || fisharea == 9 || fisharea == 10)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(1000, 0, -950), -180);
                    }
                }

                if (ballarea == 5)
                {
                    if (fisharea == 1)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-700, 0, -900), 0);
                    }
                    if (fisharea == 2)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-1450, 0, -600), 0);
                    }
                    if (fisharea == 3)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-700, 0, 900), 0);
                    }
                    if (fisharea == 4)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(1250, 0, -512), -180);
                    }
                    if (fisharea == 6)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(1250, 0, 512), -180);
                    }
                    if (fisharea == 7 || fisharea == 8)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(1000, 0, -950), -180);
                    }
                    if (fisharea == 9 || fisharea == 10)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(1000, 0, 950), -180);
                    }
                    if (fisharea == 5)
                    {
                        SelfCoutrol5(fishBody, ball, mission, ref decision, teamId, fishId, mission.TeamsRef[teamId].Fishes[fishId], ballId);
                    }
                }

                if (ballarea == 6)
                {
                    if (fisharea == 1)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-700, 0, -900), 0);
                    }
                    if (fisharea == 2)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-1450, 0, -600), 0);
                    }
                    if (fisharea == 3)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-700, 0, 900), 0);
                    }
                    if (fisharea == 6)
                    {
                        Dribble3(ref decision, mission.TeamsRef[teamId].Fishes[fishId], new Vector3(1250, 0, 512), ball);
                    }
                    if (fisharea == 5)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(1450, 0, 600), 0);
                    }
                    if (fisharea == 4)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(1450, 0, 450), 0);
                    }
                    if (fisharea == 7 || fisharea == 8 || fisharea == 9 || fisharea == 10)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(1000, 0, 950), -180);
                    }
                }*/

            }
        }
        #endregion /

        #region !志浩函数!
        /// <param name="area"> 1-左边 2-右边</param>
        public static void ZhiHaoJump(int area,Mission mission,ref Decision decision,int fishId,int teamId)
        {
            Vector3 MyFish = mission.TeamsRef[teamId].Fishes[fishId].PositionMm;
            Vector3 MyFishHead = mission.TeamsRef[teamId].Fishes[fishId].PolygonVertices[0];
            int fisharea = FishArea(MyFishHead);
            if (area==1)
            {
                if (fishId == 1)
                {
                    if (fisharea == 1)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-1550, 0, -555), (int)Math.PI);
                    }
                    if (fisharea == 2)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-1550, 0, -565), (int)Math.PI);
                    }
                    if(fisharea==3)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-700, 0, 900), (int)Math.PI);
                    }
                    if(fisharea==4)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(700, 0, -900), (int)Math.PI);
                    }
                    if(fisharea==5)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(1350, 0, -750), (int)Math.PI);
                    }
                    if(fisharea==6)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(700, 0, 900), (int)Math.PI);
                    }
                    if(fisharea==7 || fisharea==8 || fisharea==9 || fisharea==10)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-1260,0,-720), (int)Math.PI);
                    }
                }
                if (fishId == 0)
                {
                    if (fisharea == 1)
                    {
                         MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-700, 0, -900), (int)Math.PI);
                    }
                    if (fisharea == 2)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-1550, 0, 565), (int)Math.PI);
                    }
                    if (fisharea == 3)
                    {
                       MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-1550, 0, 555), (int)Math.PI);
                    }
                    if (fisharea == 4)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(700, 0, -900), (int)Math.PI);
                    }
                    if (fisharea == 5)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(1350, 0, 750), (int)Math.PI);
                    }
                    if (fisharea == 6)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(700, 0, 900), (int)Math.PI);
                    }
                    if (fisharea == 7 || fisharea == 8 || fisharea == 9 || fisharea == 10)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-1260, 0, 720), (int)Math.PI);
                    }
                }
            }

            else if (area == 2)
            {
                if (fishId == 1)
                {
                    if (fisharea == 4)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(1550, 0, -555), 0);
                    }
                    if (fisharea == 5)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(1550, 0, -565), 0);
                    }
                    if (fisharea == 6)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(700, 0, 900), 0);
                    }
                    if (fisharea == 1)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-700, 0, -900), 0);
                    }
                    if (fisharea == 2)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-1350, 0, -750), 0);
                    }
                    if (fisharea == 3)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-700, 0, 900), 0);
                    }
                    if (fisharea == 7 || fisharea == 8 || fisharea == 9 || fisharea == 10)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(1260, 0, -720),0);
                    }
                }
                if (fishId == 0)
                {
                    if (fisharea == 4)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(700, 0, -900), 0);
                    }
                    if (fisharea == 5)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(1550, 0, 565), 0);
                    }
                    if (fisharea == 6)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(1550, 0, 555), 0);
                    }
                    if (fisharea == 1)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-700, 0, -900), 0);
                    }
                    if (fisharea == 2)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-1350, 0, 750), 0);
                    }
                    if (fisharea == 3)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(-700, 0, 900), 0);
                    }
                    if (fisharea == 7 || fisharea == 8 || fisharea == 9 || fisharea == 10)
                    {
                        MoveToAreaS(mission, ref decision, teamId, fishId, new Vector3(1260, 0, 720), 0);
                    }
                }
            }
        }
        #endregion

        #region 一鱼第零次判断

        public static int Ling(xna.Vector3 p)
        {
            if ((p.X >= 290 && p.X <=600) && (p.Z <= -490&& p.Z >= -800))
            {
                return 1;
            }
            else
            {
                return 0;
            }
        }

        #endregion

        #region 一鱼第一次判断

        public static int Yi(xna.Vector3 p)
        {
            if((p.X>=1100&&p.X<=1500)&&(p.Z<=-350&&p.Z>=-1000))
            {
                return 1;
            }
            else
            {
                return 0;
            }
        }

        #endregion

        #region 一鱼第负一次判断

        public static int Fuyi(xna.Vector3 p)
        {
            if ((p.X >= 900 && p.X <= 1500) && (p.Z <= -470 && p.Z >=-1000))
            {
                return 1;
            }
            else
            {
                return 0;
            }
        }

        #endregion

        #region 一鱼第二次判断

        public static int Er(xna.Vector3 p)
        {
            if ((p.X >= 900 && p.X <= 1500) && (p.Z <= -290 && p.Z >= -1000))
            {
                return 1;
            }
            else
            {
                return 0;
            }
        }

        #endregion

        #region 一鱼第三次判断

        public static int San(xna.Vector3 p)
        {
            if ((p.X >= 1100 && p.X <= 1500) && (p.Z <= 1000 && p.Z >= 90))
            {
                return 1;
            }
            else
            {
                return 0;
            }
        }

        #endregion




        #region 零鱼第一次判断

        public static int One(xna.Vector3 p)
        {
            if ((p.X >= 900 && p.X <= 1500) && (p.Z >=400 && p.Z <=1000))
            {
                return 1;
            }
            else
            {
                return 0;
            }
        }

        #endregion

        #region 零鱼第二次判断

        public static int Two(xna.Vector3 p)
        {
            if ((p.X >= 1150 && p.X <= 1500) && (p.Z >= -70 && p.Z <=-10))
            {
                return 1;
            }
            else
            {
                return 0;
            }
        }

        #endregion

        #region 零鱼第三次判断

        public static int Three(xna.Vector3 p)
        {
            if ((p.X >= 1155 && p.X <= 1500) && (p.Z >= 510 && p.Z <=1000))
            {
                return 1;
            }
            else
            {
                return 0;
            }
        }

        #endregion

        #region 零鱼第四次判断

        public static int Four(xna.Vector3 p)
        {
            if ((p.X >= 770 && p.X <= 1500) && (p.Z <= 1000 && p.Z >=375))
            {
                return 1;
            }
            else
            {
                return 0;
            }
        }

        #endregion

        //#region 中间判断

        //public static int QQQ(xna.Vector3 p)
        //{
        //    if ((p.X >= -160 && p.X <= -80) && (p.Z <= -20 && p.Z >= 20))
        //    {
        //        return 1;
        //    }
        //    else
        //    {
        //        return 0;
        //    }
        //}

        //#endregion



    }
} 